/**
 * MOTOR 2 - RAPID FLASH DYNAMICS
 * Copyright (c) 2006-2008 Michael Baczynski http://www.polygonal.de
 * 
 * Based on Box2D by Erin Catto, http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
package de.polygonal.motor2.dynamics
{
	import de.polygonal.motor2.Constants;
	import de.polygonal.motor2.World;
	import de.polygonal.motor2.dynamics.RigidBody;
	import de.polygonal.motor2.dynamics.contact.Contact;
	import de.polygonal.motor2.dynamics.contact.solver.*;

	public class Island
	{
		public var bodies:Array;
		public var contacts:Array;
		public var joints:Array;
		
		public var bodyList:RigidBody;
		
		public var bodyCount:int;
		public var contactCount:int;
		public var jointCount:int;
		
		public var positionIterations:int;
		public var positionError:Number;
		
		public static var bodiesPerIsland:Array = [];
		public static var numIslands:int = 0;
		
		public var contactSolver:IContactSolver;
		
		public function Island()
		{
			bodyCount    = 0;
			contactCount = 0;
			jointCount   = 0;
			
			bodyList = null;
			
			bodies   = []; 
			contacts = [];
			joints   = [];
			
			contactSolver = new SIContactSolver();
			//contactSolver = new ArcadeContactSolver();
			//contactSolver = new SimpleRigidBodySolver();
		}
		
		public function init():void
		{
			bodies.length = 0;
			contacts.length = 0;
			joints.length = 0;
		}
		
		public function addBody(body:RigidBody):void
		{
			bodies[bodyCount++] = body;
		}

		public function addContact(contact:Contact):void
		{
			contacts[contactCount++] = contact;
		}

//		public function addJoint(joint:Joint):void
//		{
//			joints[jointCount++] = joint;
//		}
		
		public function clear():void
		{
			bodyCount = contactCount = jointCount = 0;
		}
		
		public function setContactSolver(csolver:IContactSolver):void
		{
			contactSolver = csolver;
		}
		
		//step:StepINfo!
		public function solve(gx:Number, gy:Number, iterations:int, dt:Number):void
		{
			var i:int, j:int;
			var b:RigidBody;
			
			/*/////////////////////////////////////////////////////////
			// INTEGRATE FORCE  
			/////////////////////////////////////////////////////////*/
			
			for (i = 0; i < bodyCount; i++)
			{
				b = bodies[i];
				if (b.invMass == 0) continue;
				
				b.vx = (b.vx + dt * (gx + b.invMass * b.fx)) * b.linDamping;
				b.vy = (b.vy + dt * (gy + b.invMass * b.fy)) * b.linDamping;
				b.w  = (b.w  + dt * (     b.invI    * b.t )) * b.angDamping;
			}
			
			/*/////////////////////////////////////////////////////////
			// SOLVE CONTACTS
			/////////////////////////////////////////////////////////*/
			
			//create contactsolver
			
			//contactSolver = new ContactSolverOrg(contacts, contactCount);
			
			//SimpleRigidBodySolver(contactSolver).setContacts(contacts);
			//SimpleRigidBodySolver(contactSolver)
			
			//ArcadeContactSolver(contactSolver).setContacts(contacts);
			//ArcadeContactSolver(contactSolver).solve();
			
			//SimpleRigidBodySolver(contactSolver).setContacts(contacts);
			//SimpleRigidBodySolver(contactSolver).solve();
			
			var solver:SIContactSolver = SIContactSolver(contactSolver);
			
			solver.setContacts(contacts, contactCount);
			solver.preStep();
			
			for (i = 0; i < jointCount; i++)
				joints[i].preSolve();
			
			for (i = 0; i < iterations; ++i)
			{
				solver.solveVelConstraints();
				
				for (j = 0; j < jointCount; j++)
					joints[j].solveVelocityConstraints(dt);
			}
			
			/*/////////////////////////////////////////////////////////
			// INTEGRATE POSITIONS
			/////////////////////////////////////////////////////////*/
			
			var s:Number, c:Number;
			for (i = 0; i < bodyCount; i++)
			{
				b = bodies[i];
				if (b.invMass == 0) continue;
				
				b.x += dt * b.vx;
				b.y += dt * b.vy;
				b.r += dt * b.w;
				
				c = Math.cos(b.r);
				s = Math.sin(b.r);
				b.r11 = c; b.r12 = -s;
				b.r21 = s; b.r22 =  c;
			}
			
			/*/////////////////////////////////////////////////////////
			// SOLVE POSITION CONSTRAINTS
			/////////////////////////////////////////////////////////*/
			
			if (World.doPositionCorrection)
			{
				var contactsOK:Boolean, jointsOK:Boolean;
				
				for (var positionIterations:int = 0; positionIterations < iterations; positionIterations++)
				{
					contactsOK = solver.solvePosConstraints(Constants.k_contactBaumgarte);
					jointsOK = true;
					for (i = 0; i < jointCount; i++)
						jointsOK = jointsOK && joints[i].solvePosConstraints();
					
					if (contactsOK && jointsOK) break;
				}
			}
			
			/*/////////////////////////////////////////////////////////
			// SYNC SHAPES, RESET FORCE ACCUMULATOR
			/////////////////////////////////////////////////////////*/
			
			for (i = 0; i < bodyCount; ++i)
			{
				b = bodies[i];
				if (b.invMass == 0) continue;
				
				b.updateShapes();
				b.fx = b.fy = b.t = 0;
			}
		}
		
		public function updateSleep(dt:Number):void
		{
			var minSleepTime:Number = Number.MAX_VALUE;

//			var linTolSqr:Number = Constants.k_linSleepToleranceSq;
//			var angTolSqr:Number = Constants.k_angSleepToleranceSq;

			var b:RigidBody, i:int;

			for (i = 0; i < bodyCount; ++i)
			{
				b = bodies[i];
				if (b.invMass == 0)
					continue;
				
				if ((b.stateBits & RigidBody.k_bitAllowSleep) == 0)
				{
					b.sleepTime = 0;
					minSleepTime = 0;
				}
				
				if ((b.stateBits & RigidBody.k_bitAllowSleep) == 0 ||
					b.w * b.w > Constants.k_angSleepToleranceSq ||
					b.vx * b.vx + b.vy * b.vy > Constants.k_linSleepToleranceSq)
				{
					b.sleepTime = 0;
					minSleepTime = 0;
				}
				else
				{
					b.sleepTime += dt;
					minSleepTime = Math.min(minSleepTime, b.sleepTime);
				}
			}
			
			if (minSleepTime >= Constants.k_timeToSleep)
			{
				for (i = 0; i < bodyCount; ++i)
				{
					b = bodies[i];
					b.stateBits |= RigidBody.k_bitSleep;
				}
			}
		}
	}
}

import de.polygonal.motor2.dynamics.RigidBody;

internal class RBNode
{
	public var body:RigidBody;
	public var next:RBNode;
}

//internal class JointNode
//{
	//public var next:Jnode;
//}

//internal class CNode
//{
	//public var next:ContactNode;
//}